#include "imu2euler.hpp"

Imu2Euler::Imu2Euler()
{   
    first_data_ = true;
    // tf_ = std::make_shared<geometry_msgs::Transform>();
    // tf_.reset(new geometry_msgs::Transform());
    broadcaster_.reset(new tf2_ros::TransformBroadcaster());

    pub_acc_euler_ = n_.advertise<geometry_msgs::PointStamped>("/acc_angle",10);
    pub_gyr_euler_ = n_.advertise<geometry_msgs::PointStamped>("/gyr_angle",10);
    pub_ch110_euler_ = n_.advertise<geometry_msgs::PointStamped>("/ch110_angle",10);

    first_q_ = true;

    first_PVQ_ = true;
    last_P_ = Eigen::Vector3d::Identity();
    cur_P_  = Eigen::Vector3d::Identity();
    last_V_ = Eigen::Vector3d::Identity();
    cur_V_  = Eigen::Vector3d::Identity();
    last_Q_ = Eigen::Vector3d::Identity();
    cur_Q_  = Eigen::Vector3d::Identity();

    n_.getParam("topic_imu", topic_imu_);
    n_.param<std::string>("topic_imu", topic_imu_ , "/imu/data");
    std::cout << "\n \033[1;32m -----> topic_imu: " << topic_imu_ << "\n \033[0m";

    sub_imu_ = n_.subscribe(topic_imu_, 10, &Imu2Euler::callback, this);

}

void Imu2Euler::callback(const sensor_msgs::ImuConstPtr& msg_imu)
{   
    // ----1 九轴的四元素姿态发布-----
    geometry_msgs::TransformStamped tf_0;
    tf_0.header.stamp = ros::Time::now();
    tf_0.header.frame_id = "map";
    tf_0.child_frame_id = "base_link";
    tf_0.transform.rotation = msg_imu->orientation;
    tf_0.transform.translation.x = 0.3;
    tf_0.transform.translation.y = 1;
    tf_0.transform.translation.z = 0.5;
    broadcaster_->sendTransform(tf_0);
    
    double roll, pitch, yaw;
    tf2::Quaternion q(msg_imu->orientation.x, msg_imu->orientation.y, msg_imu->orientation.z, msg_imu->orientation.w);
    tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);

    std::cout << std::fixed << std::setprecision(2) <<  "ch110: "
        << roll * 180 / M_PI << "\t"
        << pitch * 180 / M_PI << "\t"
        << yaw * 180 / M_PI << std::endl;

    
    msg_point_.header.frame_id = "map";
    msg_point_.header.stamp = msg_imu->header.stamp;
    msg_point_.point.x = roll;
    msg_point_.point.y = pitch;
    msg_point_.point.z = yaw;
    pub_ch110_euler_.publish(msg_point_);

    // ----2 六轴的姿态解算-----
    double ax, ay, az;
    double gx, gy, gz;
    double t;

    ax = msg_imu->linear_acceleration.x;
    ay = msg_imu->linear_acceleration.y;
    az = msg_imu->linear_acceleration.z;
    gx = msg_imu->angular_velocity.x;
    gy = msg_imu->angular_velocity.y;
    gz = msg_imu->angular_velocity.z;
    t  = msg_imu->header.stamp.toSec(); 

    acc2Euler(ax, ay, az, t);

    if(first_data_)
    {
        tf2::fromMsg(msg_imu->orientation, q0_);
        tf2::Matrix3x3(q0_).getRPY(last_roll_, last_pitch_, last_yaw_);
        last_time_ = msg_imu->header.stamp.toSec();
    }

    gyr2Euler(gx, gy, gz, t);
    

    // ------3 四元素积分---------
    if(first_q_)
    {
        // 四元素积分需要使用
        last_q_t_ = msg_imu->header.stamp.toSec();
        tf2::fromMsg(msg_imu->orientation, last_q_);
    }
    quat2Euler(gx, gy, gz, t);
    
    // 航迹推算
    if(first_PVQ_)
    {
        last_P_ << 0, 0, 0;
        last_V_ << 0, 0, 0;
        last_PVQ_t_ = msg_imu->header.stamp.toSec();
        tf2::Quaternion q_PVQ;
        tf2::fromMsg(msg_imu->orientation, q_PVQ);
        tf2::Matrix3x3(q_PVQ).getRPY(last_Q_.x(), last_Q_.y(), last_Q_.z());
    }

    Eigen::Vector3d a1(ax,ay,az);
    Eigen::Vector3d g1(gx,gy,gz);
    imu2Pose(a1, g1, t);

    std::cout << "-------------\n";



}

void Imu2Euler::acc2Euler(double ax, double ay, double az, double t)
{   
    double roll, pitch, yaw;
     // tan2对象限敏感，根据两个参数判断它是属于哪个象限并给出对应的角度值，值域范围 [-pi, pi]；
    // atan对象限不敏感，值域范围为 [-pi/2, pi/2]。
    roll = atan2(ay, az);
    pitch = -atan2(ax,sqrt(ay*ay + az*az)); // 正负号根据imu的四元素确定
    yaw = 0.;

    std::cout << std::fixed << std::setprecision(2) <<  "acc2euler: "
        << roll * 180 / M_PI << "\t"
        << pitch * 180 / M_PI << "\t"
        << yaw * 180 / M_PI << std::endl;
    
    tf2::Quaternion q;
    q.setRPY(roll, pitch, yaw);
    
    geometry_msgs::TransformStamped tf_acc;
    tf_acc.header.stamp = ros::Time::now();
    tf_acc.header.frame_id = "map";
    tf_acc.child_frame_id = "acc_link";
    tf_acc.transform.rotation.w = q.w();
    tf_acc.transform.rotation.x = q.x();
    tf_acc.transform.rotation.y = q.y();
    tf_acc.transform.rotation.z = q.z();

    tf_acc.transform.translation.x = 0.3;
    tf_acc.transform.translation.y = 1.5;
    tf_acc.transform.translation.z = 0.5;
    broadcaster_->sendTransform(tf_acc);

    msg_point_.point.x = roll;
    msg_point_.point.y = pitch;
    msg_point_.point.z = yaw;
    pub_acc_euler_.publish(msg_point_);

}

void Imu2Euler::gyr2Euler(double gx, double gy, double gz, double time)
{   
    cur_time_ = time;
    double dt = cur_time_ - last_time_;
    
    if(first_data_)
    {
        cur_roll_ = last_roll_;
        cur_pitch_ = last_pitch_;
        cur_yaw_ = last_yaw_;  
    }

    if(dt > 0.0001)  
    {
        cur_roll_  = last_roll_ + gx * dt;
        cur_pitch_ = last_pitch_ +  gy * dt;
        cur_yaw_   = last_yaw_ + gz * dt;
    }

    // 第二帧imu后, first_data_ = false, 更新上一帧的姿态角
    first_data_ = false;
    last_time_  = cur_time_;
    last_roll_  = cur_roll_;
    last_pitch_ = cur_pitch_;
    last_yaw_   = cur_yaw_;
    
    // 角度限制到-π 到 π
    if(last_pitch_ > M_PI)
        last_pitch_ += -2 * M_PI;
    if(last_pitch_ < -M_PI)
        last_pitch_ += 2 * M_PI;
    
    if(last_roll_ > M_PI)
        last_roll_ += -2 * M_PI;
    if(last_roll_ < -M_PI)
        last_roll_ += 2 * M_PI;
    
    if(last_yaw_ > M_PI)
        last_yaw_ += -2 * M_PI;
    if(last_yaw_ < -M_PI)
        last_yaw_ += 2 * M_PI;


    std::cout << std::fixed << std::setprecision(2) <<  "gyr2euler: "
        << cur_roll_ * 180 / M_PI << "\t"
        << cur_pitch_ * 180 / M_PI << "\t"
        << cur_yaw_ * 180 / M_PI << std::endl;
    
    tf2::Quaternion q;
    q.setRPY(cur_roll_,cur_pitch_, cur_yaw_);

    geometry_msgs::TransformStamped tf_gyr;
    tf_gyr.header.stamp = ros::Time::now();
    tf_gyr.header.frame_id = "map";
    tf_gyr.child_frame_id = "gyr_link";
    tf_gyr.transform.rotation.w = q.w();
    tf_gyr.transform.rotation.x = q.x();
    tf_gyr.transform.rotation.y = q.y();
    tf_gyr.transform.rotation.z = q.z();
    tf_gyr.transform.translation.x = 0.3;
    tf_gyr.transform.translation.y = 0.5;
    tf_gyr.transform.translation.z = 0.5;
    broadcaster_->sendTransform(tf_gyr);

    msg_point_.point.x = cur_roll_;
    msg_point_.point.y = cur_pitch_;
    msg_point_.point.z = cur_yaw_;
    pub_acc_euler_.publish(msg_point_);

}

void Imu2Euler::quat2Euler(double gx, double gy, double gz, double time)
{   
    cur_q_t_ = time;
    double dt = cur_q_t_ - last_q_t_;
    
    if(first_q_)
    {
        cur_q_ = last_q_;
    }
    if(dt > 0.00001)
    {   
        tf2::Quaternion dq( 0.5 * gx * dt, 0.5 * gy * dt, 0.5 * gz * dt, 1);
        cur_q_ = dq * last_q_ ;  
    }
    cur_q_.normalize();

    last_q_ = cur_q_;
    last_q_t_ = cur_q_t_;
    first_q_ = false;

    double roll1, pitch1, yaw1;
    tf2::Matrix3x3(cur_q_).getRPY(roll1, pitch1, yaw1);

    std::cout << std::fixed << std::setprecision(2) <<  "quat2euler: "
        << roll1 * 180 / M_PI << "\t"
        << pitch1 * 180 / M_PI << "\t"
        << yaw1 * 180 / M_PI << std::endl;

    geometry_msgs::TransformStamped tf_q;
    tf_q.header.stamp = ros::Time::now();
    tf_q.header.frame_id = "map";
    tf_q.child_frame_id = "quat_link";
    tf_q.transform.rotation.w = cur_q_.w();
    tf_q.transform.rotation.x = cur_q_.x();
    tf_q.transform.rotation.y = cur_q_.y();
    tf_q.transform.rotation.z = cur_q_.z();
    tf_q.transform.translation.x = 0.3;
    tf_q.transform.translation.y = -0.5;
    tf_q.transform.translation.z = 0.5;
    broadcaster_->sendTransform(tf_q);

}

void Imu2Euler::imu2Pose(Eigen::Vector3d& acc,Eigen::Vector3d& gyr, double t)
{
    cur_PVQ_t_ = t;
    double dt = cur_PVQ_t_ - last_PVQ_t_;
    
    if(first_PVQ_)
    {
        cur_P_ = last_P_;
        cur_V_ = last_V_;
        cur_Q_ = last_Q_;
    }

    if(dt > 0.0001)
    {
        // cur_Q_.x() = last_Q_.x() + dt * gyr.x();  
        cur_Q_ = last_Q_ + gyr * dt;
        cur_V_ = last_V_ + acc * dt;
        cur_P_ = last_P_ + acc * dt + 0.5 * acc * dt * dt;
    }

    first_PVQ_ = false;
    last_PVQ_t_ = cur_time_;
    last_P_ = cur_P_;
    last_V_ = cur_V_;
    last_Q_ = cur_Q_;

    std::cout << std::fixed << std::setprecision(2) << "PVQ_Quat    : " 
        << (cur_Q_ * 180. / M_PI).transpose() << std::endl;

    std::cout << std::fixed << std::setprecision(2) << "PVQ_Velocity: " 
        << (cur_V_ * 180. / M_PI).transpose() << std::endl;

    std::cout << std::fixed << std::setprecision(2) << "PVQ_Position: " 
    << (cur_P_ * 180. / M_PI).transpose() << std::endl;

    tf2::Quaternion q;
    q.setRPY(cur_Q_(0), cur_Q_(1), cur_Q_(2));
    geometry_msgs::TransformStamped tf_PVQ;
    tf_PVQ.header.stamp = ros::Time::now();
    tf_PVQ.header.frame_id = "map";
    tf_PVQ.child_frame_id = "quat_link";
    tf_PVQ.transform.rotation.w = q.w();
    tf_PVQ.transform.rotation.x = q.x();
    tf_PVQ.transform.rotation.y = q.y();
    tf_PVQ.transform.rotation.z = q.z();
    tf_PVQ.transform.translation.x = cur_P_.x();
    tf_PVQ.transform.translation.y = cur_P_.y();
    tf_PVQ.transform.translation.z = cur_P_.z();
    broadcaster_->sendTransform(tf_PVQ);

}




// int main(int argc, char** argv)
// {
//     ros::init(argc, argv, "imu2euler_node");
//     std::shared_ptr<Imu2Euler> imu2euler = std::make_shared<Imu2Euler>();
//     ros::spin();
//     return 0;
// }